'{$STAMP BS2sx} l_values VAR Byte f_values VAR Byte lvalues VAR Byte fvalues VAR Byte counter1 VAR Nib counter2 VAR Nib IR_freq VAR Word OUTPUT 12 OUTPUT 5 LOW 8 LOW 9 LOW 15 'Reset motor control HIGH 15 SEROUT 14, 240, [$80, 0, 1, 0] main: GOSUB check_sensors 'It takes 43 msec to run check_sensors once. ' debug cls, "front = ", dec fvalues, " left = ", dec lvalues ' goto main ' fvalues = 3 ' lvalues = 4 IF lvalues = 0 THEN l3 BRANCH fvalues, [side,r3,r4,r5,r5] side: BRANCH lvalues, [l2,l1,st,r1,r2] GOTO main check_sensors: l_values = 0 f_values = 0 lvalues = 0 fvalues = 0 FOR counter2 = 1 TO 4 FOR counter1 = 0 TO 3 LOOKUP counter1, [15040,14440,14180,13880], IR_freq FREQOUT 12,1, IR_freq 'Left LED drive 542 ohms l_values.LOWBIT(counter1) = ~IN10 LOOKUP counter1, [15090,14360,13900,13450], IR_freq FREQOUT 5,1, IR_freq 'Front LED drive 491 ohms f_values.LOWBIT(counter1) = ~IN7 NEXT lvalues = lvalues + NCD l_values fvalues = fvalues + NCD f_values NEXT lvalues = lvalues >> 2 fvalues = fvalues >> 2 RETURN st LOW 9 HIGH 8 SEROUT 14,240,[$80, 0, 1,127] 'right SEROUT 14,240,[$80, 0, 3,127] ' left GOTO main r1 LOW 9 HIGH 8 SEROUT 14,240,[$80, 0, 1,30] 'right SEROUT 14,240,[$80, 0, 3,127] ' left GOTO main r2 LOW 9 HIGH 8 SEROUT 14,240,[$80, 0, 1,10] 'right SEROUT 14,240,[$80, 0, 3,127] ' left GOTO main r3 LOW 8 HIGH 9 SEROUT 14,240,[$80, 0, 1,15] 'right SEROUT 14,240,[$80, 0, 3,127] ' left GOTO main r4 LOW 8 HIGH 9 SEROUT 14,240,[$80, 0, 1,10] 'right SEROUT 14,240,[$80, 0, 3,127] ' left GOTO main r5 LOW 8 HIGH 9 SEROUT 14,240,[$80, 0, 0,127] 'right SEROUT 14,240,[$80, 0, 3,0] ' left GOTO main l1 LOW 9 HIGH 8 SEROUT 14,240,[$80, 0, 1,127] 'right SEROUT 14,240,[$80, 0, 3,50] ' left GOTO main l2 LOW 9 HIGH 8 SEROUT 14,240,[$80, 0, 1,127] 'right SEROUT 14,240,[$80, 0, 3,9] ' left GOTO main l3 LOW 9 LOW 8 SEROUT 14,240,[$80, 0, 1,120] 'right SEROUT 14,240,[$80, 0, 3,20] ' left GOTO main